
#include "motor_control.h"
#include "magnetic_encoder.h"


int phase_index;

MotorControl::MotorControl(void)
{
  magnetic_encoder = new MagneticEncoder();
  pwm = new Pwm();
  angle = 0;
  duty_cycle = 0;
  old_duty_cycle = 0;
  phase_offset = 35;
  phase_delta = 10;
  phase_index = 0;
  old_phase_index = 0;
}

void MotorControl::updateAngleMeasurement(int pi)
{
  angle = magnetic_encoder->getAngle();
  //phase_index = pi;
  phase_index = int(0.11666666666666667f * (angle + phase_offset)) % 6;
  //phase_index = int((angle + phase_offset) / phase_delta) % 6;
  updatePhase();
}

int MotorControl::getAngle(void) const
{
  return angle;
}

void MotorControl::setPhaseOffset(int phase_offset)
{
  this->phase_offset = phase_offset;
  updatePhase();
}

int MotorControl::getPhaseIndex(void) const
{
  return phase_index;
}

void MotorControl::setDutyCycle(int duty_cycle)
{
  if (duty_cycle > 99) duty_cycle = 99;
  if (duty_cycle < -99)  duty_cycle = -99;
  this->duty_cycle = duty_cycle;
  //updatePhase();
}

void MotorControl::updatePhase()
{
  static long counter[6] = {0, 0, 0, 0, 0, 0};

  if (phase_index != old_phase_index ||
      duty_cycle != old_duty_cycle) {
    switch (phase_index) {
    case 0:
      pwm->setDutyCycleA( duty_cycle);
      pwm->setDutyCycleB(-duty_cycle);
      pwm->setDutyCycleC( 0.0);
      break;
    case 1:
      pwm->setDutyCycleA( duty_cycle);
      pwm->setDutyCycleB( 0.0);
      pwm->setDutyCycleC(-duty_cycle);
      break;
    case 2:
      pwm->setDutyCycleA( 0.0);
      pwm->setDutyCycleB( duty_cycle);
      pwm->setDutyCycleC(-duty_cycle);
      break;
    case 3:
      pwm->setDutyCycleA(-duty_cycle);
      pwm->setDutyCycleB( duty_cycle);
      pwm->setDutyCycleC( 0.0);
      break;
    case 4:
      pwm->setDutyCycleA(-duty_cycle);
      pwm->setDutyCycleB( 0.0);
      pwm->setDutyCycleC( duty_cycle);
      break;
    case 5:
      pwm->setDutyCycleA( 0.0);
      pwm->setDutyCycleB(-duty_cycle);
      pwm->setDutyCycleC( duty_cycle);
    }
    old_phase_index = phase_index;
    old_duty_cycle = duty_cycle;
    counter[phase_index]++;
  }
}